package com.rockwell.sniffyhunter.model;

import java.util.Map;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.MoveToNextZoneState;
import com.rockwell.sniffyhunter.utils.UnloadMazeLocatorState;

/**
 * This class provides the robot for moving to the next undiscovered zone
 * The following actions will be performed:
 * <ul>
 * <li>Move by an Offset.
 * <li>If it completes the move, it is ready to scan the next zone.
 * <li>If it does not, it might have hit a wall or an obstacle.
 * <li>If so, let's navigate around the obstacle or the wall by making a right turn (or some other preferred way of movement).
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class MoveToNextZoneObject extends StateObject {

	private IRobotSensors m_sensors;
	private Map<Integer, IDetector> m_colorDetectors;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;
	private ObstacleSkipper m_obstacleSkipper;
	private RobotCommandSequencer m_commandSequencer;
	private ExitCode m_exitCode;
	private boolean m_turnRight;
	private long startTime;
	private static final int MAX_JOG_TIME = 10000; //10 sec
	private static final int SCAN_ROW_WIDTH = 150; //mm
	private static final int REVERSE_DISTANCE = 50; //mm
	
	private enum ExitCode {
		UNINITIALIZED,
		DESTINATION_REACHED,
	}
	
	public MoveToNextZoneObject(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_colorDetectors = robot.m_colorDetectors;
		m_positionDetector = robot.m_positionDetector;
		m_sensors = robot.m_sensors;
		m_obstacleSkipper = new ObstacleSkipper(robot);
	}

	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		//m_obstacleSkipper.Initialize();
		m_turnRight = false;
		machineState = MoveToNextZoneState.START;
	}

	@Override
	public boolean IsDone() {
		return machineState == MoveToNextZoneState.DONE;
	}

	public IStateMachine start() {
		m_exitCode = ExitCode.UNINITIALIZED;
		machineState = MoveToNextZoneState.START_JOG;

		return machineState;
	}
	
	public boolean IsDestinationReached() {
		return m_exitCode == ExitCode.DESTINATION_REACHED;
	}

	/*
	public IStateMachine RotateToDestination() {
	 
		int angleToOffset = Vector2D.calculateAngleTo(
									robot.m_northernStarFinder.WhereAmI(),
									robot.m_northernStarFinder.GetCurrentZoneInfo());

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, angleToOffset);
		if (returnVal == ReturnResult.Completed) {
			log("Rotated By an offset degrees To Face Destination Point", 4);
			robot.m_northernStarFinder.Rotate(angleToOffset);
			machineState = MoveToNextZoneState.MOVE_BY_OFFSET;
		}
		return machineState;
	}
	
	public IStateMachine MoveByOffset() {
		double distanceToMove = Vector2D.calculateDistanceBetween(
										robot.m_northernStarFinder.WhereAmI(),
										robot.m_northernStarFinder.GetCurrentZoneInfo());
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveForward, distanceToMove);
		if (returnVal == ReturnResult.Completed) {
			log("Moved by an offset", 4);
			robot.m_northernStarFinder.MoveForward(distanceToMove);
			m_exitCode = ExitCode.DESTINATION_REACHED;
			machineState = MoveToNextZoneState.DONE;
		}
		else if (returnVal == ReturnResult.Aborted) { // Need to Recover from Wall or Obstacle
			log("Cannot complete the move", 4);
			// TODO: get the actual steps taken so far
			double actualStepsTakenSoFar = distanceToMove - 1;
			robot.m_northernStarFinder.MoveForward(actualStepsTakenSoFar);
			machineState = MoveToNextZoneState.FINAL_ROTATE;
		}
		return machineState;
	}

	public IStateMachine FinalRotate() {
		int remainingAngle = Vector2D.calculateAngleBetween(
									robot.m_northernStarFinder.WhereAmI(), 
									robot.m_northernStarFinder.GetCurrentZoneInfo());  
				
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, remainingAngle);
		if (returnVal == ReturnResult.Completed) {
			log("Rotated remainingAngle degrees", 4);
			robot.m_northernStarFinder.Rotate(remainingAngle);
			machineState = MoveToNextZoneState.DONE;
		}
		return machineState;
	}
	*/
	
	public IStateMachine startJog() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, 3);

		if (returnVal == ReturnResult.Completed) {
			log("startJog.MoveToNextZoneState", 4);
			machineState = MoveToNextZoneState.SCAN;
		}
		
		startTime = System.currentTimeMillis() ;
		return machineState;
	}

	public IStateMachine scan() {
		/*if (m_colorDetectors.get(DetectionIndex.OBSTACLE).isPresent() ||
			m_colorDetectors.get(DetectionIndex.MAZE1).isPresent() ||
			m_colorDetectors.get(DetectionIndex.MAZE2).isPresent() ||
			m_colorDetectors.get(DetectionIndex.MAZE3).isPresent()) {
			log("detectObstacle = true.MoveToNextZoneState", 4);
			
			machineState = MoveToNextZoneState.NAV_AWAY_FROM_OBS;
		}*/

		m_robotPosition = m_positionDetector.getPosition();
		
		if (System.currentTimeMillis() - startTime > MAX_JOG_TIME || 
				m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
			machineState = MoveToNextZoneState.STOP_JOG;
		}
		else if (!m_sensors.isWheelMoving()) {
			
			if (m_robotPosition.frontwalldetected) {
				log("frontwalldetected = true.MoveToNextZoneState", 4);
				if ((m_turnRight && m_robotPosition.rightwalldetected) ||
					(!m_turnRight && m_robotPosition.leftwalldetected)) {
					//encounter a corner wall
					m_turnRight = !m_turnRight;
					machineState = MoveToNextZoneState.REVERSE_DUE_TO_CORNER_WALL;
				}
				else {
					//encounter a play area wall
					machineState = MoveToNextZoneState.REVERSE_DUE_TO_PLAY_WALL;
				}
			}
			else {
				log("frontwalldetected = false.UnloadMazeLocator", 4);
				machineState = MoveToNextZoneState.START_JOG;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine reverseDueToCornerWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward, REVERSE_DISTANCE);
		
		if (returnVal == ReturnResult.Completed) {
			log("reverseDueToCornerWall.MoveToNextZoneState", 4);
			
			machineState = MoveToNextZoneState.TURN_DUE_TO_CORNER_PLAY_WALL_DETECTED_BY_FRONT_SIDE_SENSORS;
		}
		
		return machineState;
	}
	
	public IStateMachine reverseDueToPlayWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward, REVERSE_DISTANCE);
		
		if (returnVal == ReturnResult.Completed) {
			log("reverseDueToPlayWall.MoveToNextZoneState", 4);
			
			machineState = MoveToNextZoneState.TURN_DUE_TO_PLAY_WALL;
		}
		
		return machineState;
	}

	public IStateMachine turnDueToPlayWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(m_turnRight? RobotAction.TurnRight : RobotAction.TurnLeft);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnRightEncounterPlayWall.MoveToNextZoneState", 4);
			
			machineState = MoveToNextZoneState.SCAN_AFTER_TURN_DUE_TO_PLAY_WALL;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterTurnDueToPlayWall() {

		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.MoveToNextZoneState", 4);

			if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
				//gotcha!
				m_exitCode = ExitCode.DESTINATION_REACHED;
				machineState = MoveToNextZoneState.DONE;
			}
			else {
				machineState = MoveToNextZoneState.TURN_DUE_TO_CORNER_PLAY_WALL;
			}
		}
		else {
			log("frontwalldetected = false.MoveToNextZoneState", 4);
			machineState = MoveToNextZoneState.MOVE_TO_NEXT_SCAN_ROW;
		}
		
		return machineState;
	}
	
	public IStateMachine moveToNextScanRow() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveForward2, SCAN_ROW_WIDTH);
		
		if (returnVal == ReturnResult.Completed) {
			log("moveToNextScanRow.MoveToNextZoneState", 4);
			
			machineState = MoveToNextZoneState.SCAN_AFTER_MOVE_TO_NEXT_SCAN_ROW;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterMoveToNextScanRow() {

		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.MoveToNextZoneState", 4);

			if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
				//gotcha!
				m_exitCode = ExitCode.DESTINATION_REACHED;
				machineState = MoveToNextZoneState.DONE;
			}
			else {
				machineState = MoveToNextZoneState.TURN_DUE_TO_CORNER_PLAY_WALL;
			}
		}
		else {
			log("frontwalldetected = false.MoveToNextZoneState", 4);
			machineState = MoveToNextZoneState.TURN_AFTER_MOVE_TO_NEXT_SCAN_ROW;
		}
		
		return machineState;
	}
	
	public IStateMachine turnAfterMoveToNextScanRow() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(m_turnRight? RobotAction.TurnRight : RobotAction.TurnLeft);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnAfterMoveToNextScanRow.MoveToNextZoneState", 4);
			
			m_turnRight = !m_turnRight;
			machineState = MoveToNextZoneState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine turnDueToCornerPlayWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnBack);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnDueToCornerPlayWall.MoveToNextZoneState", 4);
			
			m_turnRight = !m_turnRight;
			machineState = MoveToNextZoneState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine turnDueToCornerPlayWallDetectedByFrontAndSideSensors() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(m_turnRight? RobotAction.TurnRight : RobotAction.TurnLeft);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnDueToCornerPlayWallDetectedByFrontAndSideSensors.MoveToNextZoneState", 4);
			
			machineState = MoveToNextZoneState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine navAwayFromObs() {

		ReturnResult returnVal = m_obstacleSkipper.PerformAction();

		if (returnVal == ReturnResult.Completed) {
			log("navAwayFromObs.MoveToNextZoneState", 4);
			machineState = MoveToNextZoneState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine stopJog() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Stop);

		if (returnVal == ReturnResult.Completed) {
			log("stopJog.MoveToNextZoneState", 4);
			m_exitCode = ExitCode.DESTINATION_REACHED;
			machineState = MoveToNextZoneState.DONE;
		}
		
		return machineState;
	}
}
